/****************************************************************************
 *
 *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
 *   Copyright (c) 2025 H743 Flight Controller Project. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include <px4_platform_common/px4_work_queue/work_queue.hpp>
#include <px4_platform_common/px4_work_queue/work_item.hpp>

// Forward declaration of debug_printf from main.c
extern "C" int debug_printf(const char *format, ...);

namespace px4
{

WorkQueue::WorkQueue(const wq_config_t &config) :
	_config(config)
{
	// Initialize intrusive list node
	sorted_list_node.sibling = nullptr;

	// Initialize runnable queue
	intrusive_queue_init(&_q, offsetof(WorkItem, queue_node));

	// Initialize attached work items list
	intrusive_sorted_list_init(&_work_items, offsetof(WorkItem, sorted_list_node), WorkItem::compare_by_name);

	// Initialize semaphores
	px4_sem_init(&_process_lock, 0, 0); // Start with 0 (no work available)
	px4_sem_setprotocol(&_process_lock, SEM_PRIO_NONE);

	px4_sem_init(&_qlock, 0, 1); // Mutex (binary semaphore)
	px4_sem_setprotocol(&_qlock, SEM_PRIO_NONE);
}

WorkQueue::~WorkQueue()
{
	// Destroy semaphores
	px4_sem_destroy(&_process_lock);
	px4_sem_destroy(&_qlock);
}

bool WorkQueue::Attach(WorkItem *item)
{
	if (item == nullptr) {
		return false;
	}

	// Lock
	do {} while (px4_sem_wait(&_qlock) != 0);

	// Add to sorted list of attached work items
	intrusive_sorted_list_add(&_work_items, item);

	// Unlock
	px4_sem_post(&_qlock);

	return true;
}

void WorkQueue::Detach(WorkItem *item)
{
	if (item == nullptr) {
		return;
	}

	// Lock
	do {} while (px4_sem_wait(&_qlock) != 0);

	// Remove from sorted list
	intrusive_sorted_list_remove(&_work_items, item);

	// Unlock
	px4_sem_post(&_qlock);
}

void WorkQueue::Add(WorkItem *item)
{
	if (item == nullptr) {
		return;
	}

	// Lock
	do {} while (px4_sem_wait(&_qlock) != 0);

	// Check if already in queue
	bool already_queued = false;
	intrusive_queue_node_t *node = (intrusive_queue_node_t *)((char *)_q.head + _q.node_offset);

	if (_q.head != nullptr) {
		void *current = _q.head;
		while (current != nullptr) {
			if (current == item) {
				already_queued = true;
				break;
			}
			intrusive_queue_node_t *current_node = (intrusive_queue_node_t *)((char *)current + _q.node_offset);
			current = (current_node->next != nullptr) ?
				(void *)((char *)current_node->next - _q.node_offset) : nullptr;
		}
	}

	if (!already_queued) {
		// Add to runnable queue
		intrusive_queue_push(&_q, item);

		// Signal worker thread
		SignalWorkerThread();
	}

	// Unlock
	px4_sem_post(&_qlock);
}

void WorkQueue::Remove(WorkItem *item)
{
	if (item == nullptr) {
		return;
	}

	// Lock
	do {} while (px4_sem_wait(&_qlock) != 0);

	// Remove from runnable queue
	intrusive_queue_remove(&_q, item);

	// Unlock
	px4_sem_post(&_qlock);
}

void WorkQueue::Clear()
{
	// Lock
	do {} while (px4_sem_wait(&_qlock) != 0);

	// Clear runnable queue
	while (!intrusive_queue_empty(&_q)) {
		intrusive_queue_pop(&_q);
	}

	// Unlock
	px4_sem_post(&_qlock);
}

void WorkQueue::SignalWorkerThread()
{
	px4_sem_post(&_process_lock);
}

/**
 * WorkQueue Run method
 */
void WorkQueue::Run()
{
	debug_printf("[WQ] %s: Run() started\n", _config.name);

	while (!_should_exit) {
		// Wait for work (blocking)
		do {} while (px4_sem_wait(&_process_lock) != 0);

		// Lock
		do {} while (px4_sem_wait(&_qlock) != 0);

		// Process all queued work items
		while (!intrusive_queue_empty(&_q)) {
			WorkItem *work = (WorkItem *)intrusive_queue_pop(&_q);

			// Unlock (allow work item to reschedule itself)
			px4_sem_post(&_qlock);

			// Execute work item
			if (work != nullptr) {
				// RunPreamble returns false if item was cancelled
				if (work->RunPreamble()) {
					work->Run();
				}
			}

			// Re-lock
			do {} while (px4_sem_wait(&_qlock) != 0);
		}

		// Unlock
		px4_sem_post(&_qlock);
	}

	debug_printf("[WQ] %s: Run() stopped\n", _config.name);
}

/**
 * Request work queue to stop
 */
void WorkQueue::request_stop()
{
	debug_printf("[WQ] %s: Stop requested\n", _config.name);
	_should_exit = true;

	// Signal to wake up the thread
	SignalWorkerThread();
}

/**
 * Print work queue status
 */
void WorkQueue::print_status(bool last)
{
	// Lock
	do {} while (px4_sem_wait(&_qlock) != 0);

	// Count attached work items
	size_t num_items = 0;
	intrusive_sorted_list_node_t *node = _work_items.head;
	while (node != nullptr) {
		num_items++;
		node = node->sibling;
	}

	debug_printf("%s (%zu items)\n", _config.name, num_items);

	// Print work items
	node = _work_items.head;
	while (node != nullptr) {
		WorkItem *item = (WorkItem *)((char *)node - offsetof(WorkItem, sorted_list_node));
		debug_printf("  - %s\n", item->ItemName());
		node = node->sibling;
	}

	// Unlock
	px4_sem_post(&_qlock);

	(void)last;
}

} // namespace px4

